#include "astar.h"

void AStar::SearchPath()
{
    m_closedset.clear();
    m_cameFrom.clear();
    m_openset.clear();
    m_gscore.clear();
    m_hscore.clear();
    m_fscore.clear();

    m_openset.insert(m_source);

    m_gscore[m_source]=0;
    m_hscore[m_source]=m_h->heuristic(m_graph->getNode(m_source)->getPos(),m_graph->getNode(m_target)->getPos());
    m_fscore[m_source]=m_gscore[m_source]+m_hscore[m_source];


    bool  isBetter=false;
    while(!m_openset.empty())
    {
        int min=1000000;
        int x=0;
        foreach(int i, m_openset)
        {
            if(m_fscore[i]<min)
            {
                min=m_fscore[i];
                x=i;
            }
        }
        min=1000000;
        if(x==m_target)
        {
            return;
        }

        m_openset.remove(x);
        m_closedset.insert(x);

        foreach(Edge* edge, m_graph->m_nodes.at(x)->edges)
        {
            if(m_closedset.find(edge->getTo()->getIndex())!=m_closedset.end())
                continue;

            double tmp= m_gscore[x] + edge->getCost();

            if(m_openset.find(edge->getTo()->getIndex())==m_openset.end())
            {
                m_openset.insert(edge->getTo()->getIndex());
                isBetter=true;
            }else if(tmp<m_gscore[edge->getTo()->getIndex()])
            {
                isBetter=true;
            }else
                isBetter=false;

            if(isBetter)
            {
                m_cameFrom.push_back(edge);

                //std::cout<<"e1 "<<edge->getFrom()->getIndex()<<" "<<edge->getTo()->getIndex()<<std::endl;
                m_gscore[edge->getTo()->getIndex()]=tmp;
                m_hscore[edge->getTo()->getIndex()]=m_h->heuristic(m_graph->getNode(edge->getTo()->getIndex())->getPos(),m_graph->getNode(m_target)->getPos());
                m_fscore[edge->getTo()->getIndex()]=m_gscore[edge->getTo()->getIndex()] + m_hscore[edge->getTo()->getIndex()];
            }

        }

    }
}
